/**
 * Copyright (c) 2024, https://www.zhz.com/
 * All rights reserved.
 * $Revision: 1.0 $
 * $Date: 2024/06/14 20:33:40 $
 * $Author: MSun $
 ******************************************************************************/

#include "zhz_drivers/driver_camera.h"
// #include "/usr/local/opencv3416/include/opencv2/core/persistence.hpp"

namespace zhz {

  DriverCamera::DriverCamera(ros::NodeHandle& nh, 
          ros::NodeHandle& pnh): img_nh_(nh), nh_(pnh){
    Init();
    ROS_INFO_STREAM("Ready to do tag detection on single images");
  }

  DriverCamera::~DriverCamera() {
    // cv::destroyWindow("view");
    ros::shutdown(); 
  }

  void DriverCamera::Run() {
    // cv::namedWindow("view");  
    // cv::startWindowThread();
    ImageCapture();
  }
  void DriverCamera::Init() {
    // image_publisher_ = nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
    std::string left_topic, right_topic, camera_info_l, camera_info_r;
    ros::param::get("~left_topic", left_topic);
    ros::param::get("~right_topic", right_topic);
    // ros::param::get("~camera_param", camera_param_);
    ros::param::get("~camera_info_l", camera_info_l);
    ros::param::get("~camera_info_r", camera_info_r);

    nh_.param("pub_camera_info", pub_camera_info_, 1);
    // image_sub_ = img_nh_.subscribe("/zhz/driver/image_raw", 10, &DriverCamera::ImageProcess, this);
    nh_.param("use_webcam", use_webcam_, true);
    nh_.param("HNY_CV_002", HNY_CV_002, true);
    nh_.param("CAMERA_INDEX", CAMERA_INDEX, 0);
    nh_.param("video_file", video_file_, std::string(""));
    nh_.param("camera_param", camera_param_, std::string(""));
    nh_.param("pub_rate", pub_rate_, 30);
    nh_.param("use_right_camera", use_right_camera_, 0);

    if (pub_camera_info_) {
      image_pub_l_ = img_nh_.advertiseCamera(left_topic, 1 );
    } else {
      camera_topic_pub_ = nh_.advertise<sensor_msgs::Image>(left_topic, 100);  
      camera_info_pub_l_ = nh_.advertise<sensor_msgs::CameraInfo>(camera_info_l, 1);
    }

    if (use_right_camera_) {
      // image_publisher_ = img_nh_.advertise(left_topic, 1 );
      image_pub_r_ = img_nh_.advertiseCamera(right_topic, 1 );
      image_pub_l_rect_ = img_nh_.advertise(left_topic, 1 );
      image_pub_r_rect_ = img_nh_.advertise(right_topic, 1 );
    }
    GetCameraInfo(cam_info_l_);
  }

  void DriverCamera::GetCameraInfo(sensor_msgs::CameraInfo &cam){
    cv::FileStorage fs_reader;

    if (!camera_param_.empty()) {
        fs_reader.open(camera_param_, cv::FileStorage::READ);
        if (!fs_reader.isOpened()) {
          ROS_ERROR_STREAM("Failed to open camera parameters file: " << camera_param_);
          return; // or handle the error as appropriate
        }
    } else {
      ROS_ERROR("Camera parameter file path is empty.");
      return; // or handle the error as appropriate
    }
    ROS_INFO_STREAM("Camera parameter read index: " << CAMERA_INDEX << "!");
    fs_reader["CameraInstrincMatL"] >> camera_intrinsic_l;
    fs_reader["CameraExtrinsicMatL"] >> camera_extrinsic_l;
    fs_reader["DistCoeffL"] >> camera_distcoeff_l;
    fs_reader["CameraInstrincMatR"] >> camera_intrinsic_r;
    fs_reader["CameraExtrinsicMatR"] >> camera_extrinsic_r;
    fs_reader["DistCoeffR"] >> camera_distcoeff_r;
    fs_reader["ImageSize"] >> image_size_;
    fs_reader["RotMatL2R"] >> RotMatL2R;
    fs_reader["TransMatL2R"] >> TransMatL2R;
    fs_reader.release();
    //左目相机矩阵
    // camera_intrinsic_l = (cv::Mat_<double>(3, 3) << 3276.85, 0, 689.805,
    //                                                 0, 3276.26, 380.782,
    //                                                 0, 0, 1);
    // //左目畸变系数
    // camera_distcoeff_l = (cv::Mat_<double>(5, 1) << -0.0271106, -3.07674, -0.00296706, 0.00423482, 161.223);
    
    // camera_intrinsic_r = (cv::Mat_<double>(3, 3) << 3280.18, 0, 640.318,
    //                                                 0, 3280.09, 390.449,
    //                                                 0, 0, 1);
    // camera_distcoeff_r = (cv::Mat_<double>(5, 1) << -0.0596768, 2.03298, 0.000210255, 0.00183348, -53.8368);
    // TransMatL2R = (cv::Mat_<double>(3, 1) <<  -179.793, 0, 0);        //T平移向量
    // RotMatL2R = (cv::Mat_<double>(3, 3) << 0.999938, 0.00220454, 0.0108998, 
    //                                       -0.00216748, 0.999992, -0.00341085,
    //                                       -0.0109072, 0.00338702, 0.999935);   //rec旋转向量
    // image_size_ = cv::Size(1280, 720);
    image_width_ = image_size_.width;
    image_height_ = image_size_.height;

    auto& d = camera_distcoeff_l;
    auto& k = camera_intrinsic_l;
    std::vector<double> D{d.at<double>(0), d.at<double>(1), d.at<double>(2), d.at<double>(3), d.at<double>(4)};
    boost::array<double, 9> K = {k.at<double>(0), k.at<double>(1), k.at<double>(2),
                                k.at<double>(3), k.at<double>(4), k.at<double>(5),
                                k.at<double>(6), k.at<double>(7), k.at<double>(8)};
    
    cv::Mat p = getOptimalNewCameraMatrix(k, d, image_size_, 0);        // get rectified projection.
    boost::array<double, 12> P = {
            p.at<double>(0,0),p.at<double>(0,1),p.at<double>(0,2),p.at<double>(0,3),
            p.at<double>(1,0),p.at<double>(1,1),p.at<double>(1,2),p.at<double>(1,3),
            p.at<double>(2,0),p.at<double>(2,1),p.at<double>(2,2),p.at<double>(2,3)};
    boost::array<double, 9> r = {1, 0, 0, 0, 1, 0, 0, 0, 1};

    std::cout << "Proj: " << p;
    cam.height = image_height_;
    cam.width = image_width_;
    cam.distortion_model = "plumb_bob";
    cam.D = D;
    cam.K = K;
    cam.P = P;
    cam.R = r;
    cam.binning_x = 0;
    cam.binning_y = 0;
    return;
}

  void DriverCamera::ImageCapture() {
    //1 捕获视频
    // Open camera with CAMERA_INDEX (webcam is typically #0).

    cv::VideoCapture capture;
    if (use_webcam_) {
      ROS_INFO_STREAM("use_webcam_: " << use_webcam_ << "!");
      capture.open(CAMERA_INDEX);
      // capture.open(CAMERA_INDEX, cv::CAP_V4L2);
      // capture.open(CAMERA_INDEX, cv::CAP_GSTREAMER);
      // capture.open(CAMERA_INDEX,cv::CAP_DSHOW);
      // capture.open(CAMERA_INDEX,cv::CAP_MSMF);
      ROS_INFO_STREAM("debug: 0"  << "!");
      capture.set(cv::CAP_PROP_FRAME_WIDTH, image_width_ * 2);
      capture.set(cv::CAP_PROP_FRAME_HEIGHT, image_height_);
      
    } else {
      ROS_INFO_STREAM("Open camera with video_file: " << video_file_ << "!");
      capture.open(video_file_);
    }
    if(not capture.isOpened()) {
      ROS_ERROR_STREAM(
        "Failed to open camera with index " << CAMERA_INDEX << "!");
      ROS_ERROR_STREAM(
        "Failed to open camera with video_file " << video_file_ << "!");
      ros::shutdown();
    } else {
      ROS_INFO_STREAM("Open camera with index: " << CAMERA_INDEX << "!");
    }

    // 获取摄像头的帧率
    double fps = capture.get(cv::CAP_PROP_FPS);
    cv::Mat mapLx, mapLy, mapRx, mapRy;
    if (use_right_camera_) {
    cv::Mat rectifyImageL, rectifyImageR;
    cv::Mat Rl, Rr, Pl, Pr, Q, NewCameraMatrix_;
    cv::Rect validROIL;
    cv::Rect validROIR;
    cv::stereoRectify(camera_intrinsic_l, camera_distcoeff_l, 
      camera_intrinsic_r, camera_distcoeff_r, image_size_, 
      RotMatL2R, TransMatL2R, Rl, Rr, Pl, Pr, Q, cv::CALIB_ZERO_DISPARITY,
      0, image_size_, &validROIL, &validROIR);
    cv::initUndistortRectifyMap(camera_intrinsic_l, camera_distcoeff_l, 
                                Rl, Pl, image_size_, CV_32FC1, mapLx, mapLy);
    cv::initUndistortRectifyMap(camera_intrinsic_r, camera_distcoeff_r, 
                            Rr, Pr, image_size_, CV_32FC1, mapRx, mapRy);
    }
    ROS_INFO_STREAM("debug: 1"  << "!");

    // cv::initUndistortRectifyMap(camera_intrinsic_l, camera_distcoeff_l, cv::Mat(),
    //   NewCameraMatrix_, image_size_, CV_32FC1, mapLx, mapLy);
    //cv_bridge功能包提供了ROS图像和OpenCV图像转换的接口，建立了一座桥梁
    cv::Mat capture_frame;
    cv_bridge::CvImagePtr frame = boost::make_shared<cv_bridge::CvImage >();
    cv_bridge::CvImagePtr frame_L = boost::make_shared<cv_bridge::CvImage >();
    cv_bridge::CvImagePtr frame_R = boost::make_shared<cv_bridge::CvImage >();
    cv_bridge::CvImagePtr frame_L_rect = boost::make_shared<cv_bridge::CvImage >();
    cv_bridge::CvImagePtr frame_R_rect = boost::make_shared<cv_bridge::CvImage >();
    frame->encoding = sensor_msgs::image_encodings::BGR8;
    frame_L->encoding = sensor_msgs::image_encodings::BGR8;
    frame_R->encoding = sensor_msgs::image_encodings::BGR8;
    frame_L_rect->encoding = sensor_msgs::image_encodings::BGR8;
    frame_R_rect->encoding = sensor_msgs::image_encodings::BGR8;
    frame->header.seq = 0;
    ROS_INFO_STREAM("debug: 2"  << "!");
    // ros::Rate rate_20hz(pub_rate_); // 20Hz;
    ros::Rate rate_20hz(pub_rate_); // 20Hz;
    
    while(ros::ok()) {
      // ROS_INFO_STREAM("debug: 3"  << "!");
      auto camera_time_1 = ros::Time::now().toSec();
      std::chrono::high_resolution_clock::time_point time_start =
                                    std::chrono::system_clock::now();
      capture >> capture_frame; //流的转换
      frame->image = capture_frame;
      if(frame->image.empty())
      {
        ROS_ERROR_STREAM( "Failed to capture frame!" );
        capture.open(CAMERA_INDEX);
        rate_20hz.sleep();
        continue;
        // ros::shutdown();
      } else {
        ROS_INFO_STREAM("frame->image: update"  << "!");
      }
      std::cout << "debug 3" <<std::endl;
      frame_L->image = capture_frame(cv::Rect(0, 0, image_width_, image_height_));
      frame_R->image = capture_frame(cv::Rect(image_width_, 0, image_width_, image_height_));
      if (!HNY_CV_002) std::swap(frame_L->image, frame_R->image);
      //打成ROS数据包
      frame->header.stamp = ros::Time::now();
      frame->header.frame_id = "zhz_camera";
      frame->header.seq +=1;
      frame_L->header = frame->header;
      cam_info_l_.header = frame->header;
      std::cout << "debug 4" <<std::endl;
      if (pub_camera_info_) {
        image_pub_l_.publish(*frame_L->toImageMsg(), cam_info_l_);
      } else {
        camera_info_pub_l_.publish(cam_info_l_);
        camera_topic_pub_.publish(*frame_L->toImageMsg());
      }
      std::cout << "debug 5" <<std::endl;

      if (use_right_camera_) {
        frame_R->header = frame->header;
        frame_R_rect->header = frame->header;
        frame_R_rect->header = frame->header;
        // image_publisher_.publish(frame->toImageMsg());
        image_pub_r_.publish(*frame_R->toImageMsg(), cam_info_r_);
        cv::remap(frame_L->image, frame_L_rect->image, mapLx, mapLy, cv::INTER_LINEAR);
        cv::remap(frame_R->image, frame_R_rect->image, mapRx, mapRy, cv::INTER_LINEAR);
        image_pub_l_rect_.publish(frame_L_rect->toImageMsg());
        image_pub_r_rect_.publish(frame_R_rect->toImageMsg());
      }
      // auto camera_time_6 = ros::Time::now().toSec();
      // cv::waitKey( 3 );//opencv刷新图像 3ms
      // std::cout << "Time Cost ros: " << camera_time_6 - camera_time_1 << " s" << std::endl;
      // std::chrono::high_resolution_clock::time_point time_end =
      //                               std::chrono::system_clock::now();
      // std::chrono::duration<double> time_cost =
      //     std::chrono::duration_cast<std::chrono::duration<double>>(
      //                                time_end - time_start);
      
      // std::cout << "Time Cost chrono: " << time_cost.count() * 1000 << " ms" <<std::endl;
      // auto sleep_time = 50 - time_cost.count() * 1000;
      // sleep_time = sleep_time > 0 ? sleep_time : 0;
      // sleep(sleep_time * 0.001);
      std::cout << "debug 6" <<std::endl;
      // ros::spinOnce();
      std::cout << "debug 7" <<std::endl;
      rate_20hz.sleep();
      ROS_INFO_STREAM("frame->header.seq %d "  << frame->header.seq);
    }
    capture.release();  //释放流
    return;
  }

  void DriverCamera::ImageProcess(const sensor_msgs::ImageConstPtr& msg) {
  //sensor_msgs::Image ROS中image传递的消息形式
    try {  
      cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);  
    // cv::WaitKey(3);  
    } catch (cv_bridge::Exception& e) {  
      ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());  
    }  
    // return;
  }
} // namespace zhz



